#include <string.h>
#include <stdlib.h>

#include "hcpp_config.h"
#include "hcpp/ptp2_log.h"
#include "spi/spi_master.h"
#include "spi/spi_master_driver.h"
#include "hcpp/ptp2_task_queue.h"

static bool spi_master_task_timeout(void *ref, void *param);
static void spi_master_msg_queues_init(spi_msg_queue *queue);
static void spi_master_msg_queue_resize(spi_msg_queue *queue);

/*
static void spi_master_destructor(void *ptr);

static class_info_t class_info = {
    .size        = sizeof(spi_master_t),
    .destructor  = spi_master_destructor,
    .mem_alloc   = NULL,
    .mem_free    = NULL
};
*/


bool
spi_master_init(spi_master_t *spi_master)
{

    /* init object */
    //object_init(spi_master, &class_info);

    /* init timeout */
    if (!ptp2_task_init(&(spi_master->task_timeout), "SPI master timeout", spi_master_task_timeout, spi_master, NULL)) {
        return false;
    }

    ptp2_task_queue_add(PTP2_TASK_QUEUE(), PTP2_TASK_QUEUE_RECURRENT, &(spi_master->task_timeout));

    // initialize input queues
    spi_master_msg_queues_init(&spi_master->queues[SPI_LO_PRIO_IN_QUEUE]);
    spi_master_msg_queues_init(&spi_master->queues[SPI_HI_PRIO_IN_QUEUE]);

    // initialize driver
    spi_driver_init(&spi_master->driver);

    spi_master->frame_builder = spi_frame_encode;



    return true;
}



/**
 * The function append an spi_message to the spi_master internal queues
 * according to the message priority
 *
 * @param   spi_master: ref
 * @param   message: input ref to the message to send
 * @param   prio: input priority associated to the message
 * @return  bool true if successful
 *
 */
bool
spi_master_add_message(spi_master_t *spi_master, spi_frame_t *message, uint8_t prio)
{


  if(prio < member_size(spi_master_t, queues)/ member_size(spi_master_t, queues[0]) ){
    if(spi_master->queues[prio].len < SPI_MESSAGE_QUEUE_MAX_LEN){
      memcpy(&spi_master->queues[prio].msgs[spi_master->queues[prio].len], message, sizeof(spi_frame_t));
      spi_master->queues[prio].len++;
      return true;
    }
  }

  return false;
}



bool
spi_queue_ready(spi_master_t *spi_master, uint8_t prio)
{
  if(spi_master->queues[prio].len < SPI_MESSAGE_QUEUE_MAX_LEN-1){
    return true;
  }
  return false;

}





/**
 * spi_master task routine called periodically.
  *
 * @param   spi_master ref
 * @param   param input parameter
 *
 * @return  NULL if no message has to be sent,
 *          otherwise a pointer to the next message to send
 */
static bool
spi_master_task_timeout(void *ref, void *param)
{
#define MAX_DELAY_CNT	1000

  bool          driver_result=false;
  bool          result = false;
  spi_frame_t   *spi_msg = NULL;

  if(NULL != ref){
    spi_master_t *spi_master = (spi_master_t *) ref;

    // if no command being served
    if(spi_master->current_cmd.status== status_empty){

      // peek a message from input queues if any is waiting to be sent
      if( NULL != (spi_msg = spi_master_msg_get_next(spi_master)) ){
        spi_master->current_cmd.status=status_pending;

        result = spi_driver_send_message(&spi_master->driver, spi_msg);
        if(false == result){
          // TODO
        }
      }
    }

    // let the spi_driver run
    driver_result=spi_driver_fsm_run(&spi_master->driver);
    if(true == driver_result && spi_master->current_cmd.status== status_pending){

      // if transmission was successful, remove message from queue
      spi_master_msg_queue_resize(&spi_master->queues[spi_master->current_queue]);

      // if the driver returns true and SPI master was processing
      if(spi_master->current_cmd.status==status_pending){
        spi_master->current_cmd.status=status_done;
      }
    }
    else{

    }
  }

  return result;
}





/**
 * The function polls the spi_master queues and return
 * the most prior spi_msg. The returned message is not
 * removed from the queue. This has to be done by the
 * user once the message has been actually transmitted.
 *
 * @param   spi_master ref
 * @return  NULL if no message has to be sent,
 *          otherwise a pointer to the next message to send
 */
spi_frame_t* spi_master_msg_get_next(spi_master_t *spi_master)
{
  spi_frame_t *p_spi_msg = NULL;
  int8_t        chosen = -1;

  // TODO: add policy to avoid serving always the same queue
  if(spi_master->queues[SPI_HI_PRIO_IN_QUEUE].len > 0){
    chosen = SPI_HI_PRIO_IN_QUEUE;
  }
  else if(spi_master->queues[SPI_LO_PRIO_IN_QUEUE].len > 0){
    chosen = SPI_LO_PRIO_IN_QUEUE;
  }


  if(chosen != -1){
    p_spi_msg = &spi_master->queues[chosen].msgs[0];
  }

  return p_spi_msg;

}

void spi_master_msg_queues_init(spi_msg_queue *queue)
{
  int i;
  for(i=1; i < SPI_MESSAGE_QUEUE_MAX_LEN; i++){
    memset(&queue->msgs[i-1], 0x00, sizeof(spi_frame_t));
  }
  queue->len=0;
}

void
spi_master_msg_queue_resize(spi_msg_queue *queue)
{

  if(NULL != queue){
    int i;

    for(i=1; i < queue->len; i++){
      queue->msgs[i-1] = queue->msgs[i];
    }
    memset(&queue->msgs[i], 0x00, sizeof(spi_frame_t));
  }
}

/*
static void
spi_master_destructor(void *ptr)
{
  spi_master_t *this = (spi_master_t *) ptr;

    object_release(&(this->task_timeout));

}
*/
// EOF




